/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.femto.math.linalg;

/*
 *  PORTED FROM:
 *
    Copyright (c) 2000 Cass Everitt
        Copyright (c) 2000 NVIDIA Corporation
    All rights reserved.

    Redistribution and use in source and binary forms, with or
        without modification, are permitted provided that the following
        conditions are met:

     * Redistributions of source code must retain the above
           copyright notice, this list of conditions and the following
           disclaimer.

     * Redistributions in binary form must reproduce the above
           copyright notice, this list of conditions and the following
           disclaimer in the documentation and/or other materials
           provided with the distribution.

     * The names of contributors to this software may not be used
           to endorse or promote products derived from this software
           without specific prior written permission.

       THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
           ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
           LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
           FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
           REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
           INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
           BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
           LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
           CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
           LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
           ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
           POSSIBILITY OF SUCH DAMAGE.


    Cass Everitt - cass@r3.nu
*/

/**
 *
 * @author root
 */
public class Quat {
    public double[] q = new double[4];

    public Quat() {
        setZero();
    }

    public Quat(Quat p) {
        set(p);
    }

    public Quat(double q0, double q1, double q2, double q3) {
        set(q0, q1, q2, q3);
    }

    public Quat(double s, Vec3 u) {
        set(s, u);
    }

    public double getScalar() {
        return q[1];
    }

    public Vec3 getVector() {
        return new Vec3(q[1], q[2], q[3]);
    }

    public void set(double[] a) {
        for(int i = 0; i < 4; i++) {
            q[i] = a[i];
        }
    }

    public void set(double q0, double q1, double q2, double q3) {
        q[0] = q0;
        q[1] = q1;
        q[2] = q2;
        q[3] = q3;
    }
    
    public void set(double s, Vec3 u) {
        q[0] = s;
        q[1] = u.v[0];
        q[2] = u.v[1];
        q[3] = u.v[2];
    }

    public void set(Quat p) {
        for(int i = 0; i < 4; i++) {
            q[i] = p.q[i];
        }
    }

    public void setZero() {
        for(int i = 0; i < 4; i++) {
            q[i] = 0.0;
        }
    }

    public void setFormAxisRadians(Vec3 axis, double theta) {
        double sqnorm = axis.norm2();

        if(sqnorm == 0.0) {
            // axis too small.
            q[0] = q[1] = q[2] = 0.0;
            q[3] = 1.0;
        }
        else {
            theta *= 0.5;
            double sin_theta = Math.sin(theta);

            if ( sqnorm != 1.0) {
                sin_theta /= Math.sqrt(sqnorm);
            }
            
            q[0] = sin_theta * axis.v[0];
            q[1] = sin_theta * axis.v[1];
            q[2] = sin_theta * axis.v[2];
            q[3] = Math.cos(theta);
        }
    }

    public void setFromMatrix(Matrix4x4 A) {
        double tr, s;
        int i, j, k;
        final int[] nxt = { 1, 2, 0 };

        tr = A.m(0,0) + A.m(1,1) + A.m(2,2);

        if(tr > 0.0) {
            s = Math.sqrt( tr + A.m(3,3) );
            q[3] = s * 0.5;
            s = 0.5 / s;

            q[0] = ( A.m(1,2) - A.m(2,1) ) * s;
            q[1] = ( A.m(2,0) - A.m(0,2) ) * s;
            q[2] = ( A.m(0,1) - A.m(1,0) ) * s;
        }
        else {
            i = 0;
            if(A.m(1,1) > A.m(0,0)) {
                i = 1;
            }

            if(A.m(2,2) > A.m(i,i)) {
                i = 2;
            }

            j = nxt[i];
            k = nxt[j];

            s = Math.sqrt( ( A.m(i,j) - ( A.m(j,j) + A.m(k,k) )) + 1.0 );

            q[i] = s * 0.5;
            s = 0.5 / s;

            q[3] = ( A.m(j,k) - A.m(k,j) ) * s;
            q[j] = ( A.m(i,j) + A.m(j,i) ) * s;
            q[k] = ( A.m(i,k) + A.m(k,i) ) * s;
        }
    }

    public void setFromEndpoints(Vec3 rotateFrom, Vec3 rotateTo) {
        double alpha;

        Vec3 p1 = rotateFrom.normalized();
        Vec3 p2 = rotateTo.normalized();

        alpha = Vec3.dot(p1, p2);

        if( alpha == 1.0 ) {
            setZero();
            return;
        }

        // ensures that the anti-parallel case leads to a positive dot
        if( alpha == -1.0) {
            Vec3 v;

            if(p1.v[0] != p1.v[1] || p1.v[0] != p1.v[2]) {
                v = new Vec3(p1.v[1], p1.v[2], p1.v[0]);
            }
            else {
                v = new Vec3(-p1.v[0], p1.v[1], p1.v[2]);
            }

            v.maddInplace(p1, -Vec3.dot(p1, v));
            v.normalizeSelf();

            setFormAxisRadians(v, Math.PI);
            return;
        }

        p1 = Vec3.cross(p1, p2);

        setFormAxisRadians(p1, Math.acos(alpha));
    }

    public void mulInplace(Quat p) {
        Quat r = new Quat(this);

        q[3] = r.q[3] * p.q[3] - r.q[0] * p.q[0] - r.q[1] * p.q[1] - r.q[2] * p.q[2];
        q[0] = r.q[3] * p.q[0] + r.q[0] * p.q[3] + r.q[1] * p.q[2] - r.q[2] * p.q[1];
        q[1] = r.q[3] * p.q[1] + r.q[1] * p.q[3] + r.q[2] * p.q[0] - r.q[0] * p.q[2];
        q[2] = r.q[3] * p.q[2] + r.q[2] * p.q[3] + r.q[0] * p.q[1] - r.q[1] * p.q[0];
    }

    public static Quat mul(Quat r, Quat p) {
        Quat s = new Quat(r);
        s.mulInplace(p);
        return s;
    }

    public void normalizeSelf() {
        double invNorm = 1.0/Math.sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
        q[0] *= invNorm;
        q[1] *= invNorm;
        q[2] *= invNorm;
        q[3] *= invNorm;
    }

    public Quat conjugate() {
        return new Quat(-q[0], -q[1], -q[2], q[3]);
    }

    public Vec3 mul(Vec3 u) {
        Vec3 result = new Vec3();
        double v_coef = q[3] * q[3] - q[0] * q[0] - q[1] * q[1] - q[2] * q[2];
        double u_coef = 2.0 * (u.v[0] * q[0] + u.v[1] * q[1] + u.v[2] * q[2]);
        double c_coef = 2.0 * q[3];

        result.v[0] = v_coef * u.v[0] + u_coef * q[0] + c_coef * (q[1] * u.v[2] - q[2] * u.v[1]);
        result.v[1] = v_coef * u.v[1] + u_coef * q[1] + c_coef * (q[2] * u.v[0] - q[0] * u.v[2]);
        result.v[2] = v_coef * u.v[2] + u_coef * q[2] + c_coef * (q[0] * u.v[1] - q[1] * u.v[0]);
        return result;
    }

    public void mulAngle(double factor) {
        setFormAxisRadians(getAxis(), getRadians()*factor);
    }

    public double getRadians() {
        return Math.acos( q[3] ) * 2.0;
    }

    public Vec3 getAxis() {
        Vec3 axis = new Vec3();
        if ( getRadians() == 0.0 ) {
            axis.set( 0.0, 0.0, 1.0 );
        }
        else {
            axis = new Vec3(q[0], q[1], q[2]);
            axis.normalizeSelf();
        }
        return axis;
    }

    public Matrix4x4 toMatrix4x4() {
        Matrix4x4 A = new Matrix4x4();

        double s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
        double norm = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];

        s = ( norm == 0.0) ? 0.0 : 2.0 / norm;

        xs = q[0] * s;
        ys = q[1] * s;
        zs = q[2] * s;

        wx = q[3] * xs;
        wy = q[3] * ys;
        wz = q[3] * zs;

        xx = q[0] * xs;
        xy = q[0] * ys;
        xz = q[0] * zs;

        yy = q[1] * ys;
        yz = q[1] * zs;
        zz = q[2] * zs;

        A.m(0,0, 1.0 - ( yy + zz ));
        A.m(1,0, xy + wz);
        A.m(2,0, xz - wy);

        A.m(0,1, xy - wz);
        A.m(1,1, 1.0 - ( xx + zz ));
        A.m(2,1, yz + wx);

        A.m(0,2, xz + wy);
        A.m(1,2, yz - wx);
        A.m(2,2, 1.0 - ( xx + yy ));

        A.m(3,0, 0);
        A.m(3,1, 0);
        A.m(3,2, 0);
        A.m(0,3, 0);
        A.m(1,3, 0);
        A.m(2,3, 0);
        A.m(3,3, 1.0);

        return A;
    }

    public String toString() {
        return new String("[" + q[0] + " " + q[1] + " " + q[2] + " " + q[3] + "]");
    }

    public static Quat slerp(Quat a, Quat b, double alpha) {
        Quat r = new Quat();

        double cos_omega = a.q[0] * b.q[0] + a.q[1] * b.q[1] + a.q[2] * b.q[2] + a.q[3] * b.q[3];
        // if B is on opposite hemisphere from A, use -B instead

        boolean bflip = cos_omega < 0.0;
        if( bflip ) {
            cos_omega = -cos_omega;
        }

        // complementary interpolation parameter
        double beta = 1.0 - alpha;

        if(cos_omega >= 1.0) {
            return a;
        }

        double omega = Math.acos(cos_omega);
        double one_over_sin_omega = 1.0 / Math.sin(omega);

        beta    = Math.sin(omega*beta)  * one_over_sin_omega;
        alpha   = Math.sin(omega*alpha) * one_over_sin_omega;

        if(bflip) {
            alpha = -alpha;
        }

        r.q[0] = beta * a.q[0]+ alpha * b.q[0];
        r.q[1] = beta * a.q[1]+ alpha * b.q[1];
        r.q[2] = beta * a.q[2]+ alpha * b.q[2];
        r.q[3] = beta * a.q[3]+ alpha * b.q[3];
        return r;
    }
}
